package com.lego.jbv;

import lejos.nxt.*;
/**
 * A LEGO 9797 car with a sonar sensor. The sonar is used
 * to maintain the car at a constant distance 
 * to objects in front of the car.
 * 
 * The sonar sensor should be connected to port 1. The
 * left motor should be connected to port C and the right 
 * motor to port B.
 * 
 * @author  Ole Caprani
 * @version 24.08.08
 */
public class Tracker2
{
  public static void main (String[] aArg)
  throws Exception
  {
     UltrasonicSensor us = new UltrasonicSensor(SensorPort.S4);
     final int  noObject = 255;
     int distance,				//Measured variable
         desiredDistance = 35, 	//cm
         power = 0, 			//Controlled variable
         minPower = 60;
     float error, previousError = 0, integral = 0, derivative = 0, 
    		 Kp = (float)2f, 
    		 Ki = (float)0.0007, 
    		 Kd = (float)80, 
    		 dt = (float)50;
	  
     LCD.drawString("Distance: ", 0, 1);
     LCD.drawString("Power:    ", 0, 2);
	   
     while (!Button.ESCAPE.isDown())
     {		   
         distance = us.getDistance();
		 
         //If an objects is found
         if (distance != noObject) 
         {
             error = distance - desiredDistance;
             integral = integral + error*dt;
             derivative = (error - previousError)/dt;
             power = Math.round(Kp * error + Ki * integral + Kd * derivative);
             previousError = error;
             
             //If distance is too long - move forward
             if ( error > 0 )
             { 
                 //power = Math.min(minPower + power,100);
                 Car.forward(power,power);
                 LCD.drawString("Forward ", 0, 3);
             }
             
             //Else is the distance too short - move backward
             else 
             {
                 //power = Math.min(minPower + Math.abs(power),100);
                 Car.backward(power, power);
		         LCD.drawString("Backward", 0, 3);
		    	 
             }
             
             LCD.drawInt(distance,4,10,1);
             LCD.drawInt(power, 4,10,2);
		 }
         
         //No object found - Move forward at full speed
         else
             Car.forward(100, 100);
		 
         Thread.sleep((long)dt);
     }
	 
     Car.stop();
     LCD.clear();
     LCD.drawString("Program stopped", 0, 0);
     Thread.sleep(2000);
   }
}

